package edu.mapi.aamas.behaviors;

import jade.core.Agent;
import jade.core.behaviours.TickerBehaviour;

import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import edu.mapi.aamas.common.VehicleState;
import edu.mapi.aamas.uav.UavAgent;
/**
 * @author ZP
 */
public class Navigation extends TickerBehaviour {

	private double multiplier = 1.0;
	
	public Navigation(Agent a, long period) {
		super(a, period);
		multiplier = (double)period/1000.0;
	}
	
	protected void onTick() {
		if (myAgent instanceof UavAgent) {
			UavAgent uav = (UavAgent)myAgent;
			
			VehicleState physicalState = uav.getPhysicalState();			
			
			while(uav.getPendantTasks().size() > 0) {
				Point3d destination = uav.getPendantTasks().get(0).getPoints().get(uav.getPendantTasks().get(0).getCurrentPoint());
				destination.z = uav.getPhysicalState().getPoint().z;
				//System.err.println(	uav.getPhysicalState().getPoint().distance(destination));
				if (uav.getPhysicalState().getPoint().distance(destination) < 30)
				{
					uav.getPendantTasks().remove(0);
//					System.err.println("##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							"##############################################\n" +
//							uav.getPhysicalState().getPoint().distance(destination));
				}
				else
					break;
			}
			
			if (uav.getPendantTasks().size() == 0)
				uav.getPhysicalState().setDestination(null);
			
			if (uav.getPendantTasks().size() > 0) {
				Point3d destination = uav.getPendantTasks().get(0).getPoints().get(uav.getPendantTasks().get(0).getCurrentPoint());
			
				
				
				physicalState.setDestination(destination);
				double curAngle = uav.getPhysicalState().getYaw();
				Point3d curPoint = uav.getPhysicalState().getPoint();

				//System.out.println("Ang="+Math.toDegrees(Math.atan2(destination.y-curPoint.y, destination.x-curPoint.x)));
				
				//go from (curPoint,curAngle) to destination
				Vector3d vecDest=new Vector3d(destination.x-curPoint.x,destination.y-curPoint.y,0);
				vecDest.normalize();
				
				Vector3d vecFront=new Vector3d(Math.cos(curAngle),Math.sin(curAngle),0);
				
				//Vector3d vecRight=new Vector3d(Math.cos(curAngle+Math.PI/2),Math.sin(curAngle),0);
				//Vector3d vecLeft=new Vector3d(Math.cos(curAngle-Math.PI/2),Math.sin(curAngle),0);
				
				Vector3d vecRight=new Vector3d(vecFront.y,-vecFront.x,0);
				Vector3d vecLeft=new Vector3d(-vecFront.y,vecFront.x,0);
				
				double angle=vecFront.angle(vecDest);
				
				if(multiplier*uav.getMaxYaw()<angle) angle=uav.getMaxYaw()*multiplier;
				
				if(vecRight.angle(vecDest)<vecLeft.angle(vecDest))
					uav.getPhysicalState().setYaw(curAngle-angle);
				else
					uav.getPhysicalState().setYaw(curAngle+angle);
				
				//Andar para a frente
				curAngle = uav.getPhysicalState().getYaw();
				
				vecFront=new Vector3d(Math.cos(curAngle),Math.sin(curAngle),0);
				
				vecFront.normalize();
				
				vecFront.scale(uav.getCruiseVel() * multiplier);
				
				//diminuir a fuel os metros que andou 
				double fuel = uav.getPhysicalState().getFuel();
				fuel-=multiplier*uav.getCruiseVel()*uav.getLitersSpeedRate();
				uav.getPhysicalState().setFuel(fuel);
				
				curPoint.x+=vecFront.x;
				curPoint.y+=vecFront.y;
				//curPoint.z+=vecFront.z;
				
				uav.getPhysicalState().setPoint(curPoint);
				
				
				//vecFront.
				
				/*
				p.scale(curPoint.distance(destination));
				Vector3d vec=new Vector3d();
				vec.
				*/
			}
			else {
				// LOITER
				double curAngle = uav.getPhysicalState().getYaw();
				Point3d curPoint = uav.getPhysicalState().getPoint();

				//rodar 
				Vector3d vecFront=new Vector3d(Math.cos(curAngle+uav.getMaxYaw()),Math.sin(curAngle+uav.getMaxYaw()),0);
				uav.getPhysicalState().setYaw(curAngle+uav.getMaxYaw()*multiplier);
				
				// andar para a frente
				curAngle = uav.getPhysicalState().getYaw();
				
				vecFront=new Vector3d(Math.cos(curAngle),Math.sin(curAngle),0);
				
				vecFront.normalize();
				
				vecFront.scale(uav.getMinVel() * multiplier);
				
				//diminuir a fuel os metros que andou 
				double fuel = uav.getPhysicalState().getFuel();
				fuel-=multiplier*uav.getMinVel()*uav.getLitersSpeedRate();
				uav.getPhysicalState().setFuel(fuel);


				curPoint.x+=vecFront.x;
				curPoint.y+=vecFront.y;
				//curPoint.z=10;
				
				uav.getPhysicalState().setPoint(curPoint);
			}
			uav.stateUpdated();
		}
	}
}
